#define WINDUP_GUARD_GAIN 4.0
 
float iState = 0;
 float lastTemp = 0;
 
float pgain;
 float igain;
 float dgain;
 
float pTerm, iTerm, dTerm; 

int pgainAddress, igainAddress, dgainAddress;
 
void setupPID(unsigned int padd, int iadd, int dadd) {
 
  pgain = padd;
   igain = iadd;
   dgain = dadd;
 }
 
 
float updatePID(float targetTemp, float curTemp, struct PID *p)
 {
   // these local variables can be factored out if memory is an issue, 
  // but they make it more readable
   double result;
   float error;
   float windupGaurd;
 
  // determine how badly we are doing
   error = targetTemp - curTemp;
 
  // the pTerm is the view from now, the pgain judges 
  // how much we care about error we are this instant.
   pTerm = pgain * error;
 
  // iState keeps changing over time; it's 
  // overall "performance" over time, or accumulated error
   iState += error;
 
  // to prevent the iTerm getting huge despite lots of 
  //  error, we use a "windup guard" 
  // (this happens when the machine is first turned on and
   // it cant help be cold despite its best efforts)
 
  // not necessary, but this makes windup guard values 
  // relative to the current iGain
   windupGaurd = WINDUP_GUARD_GAIN / igain;  
 
  if (iState > windupGaurd) 
    iState = windupGaurd;
   else if (iState < -windupGaurd) 
    iState = -windupGaurd;
   iTerm = igain * iState;
 
  // the dTerm, the difference between the temperature now
   //  and our last reading, indicated the "speed," 
  // how quickly the temp is changing. (aka. Differential)
   dTerm = (dgain* (curTemp - lastTemp));
 
  // now that we've use lastTemp, put the current temp in
   // our pocket until for the next round
   lastTemp = curTemp;
 
  // the magic feedback bit
   return  pTerm + iTerm - dTerm;
 }
 
void PIDsetup(){
  
  pid[ZAXIS_PID_IDX].P = 200.0;
  pid[ZAXIS_PID_IDX].I = 5.0;
  pid[ZAXIS_PID_IDX].D = 0.0;
  pid[ATTITUDE_GYRO_XAXIS_PID_IDX].P = 10.0;
  pid[ATTITUDE_GYRO_XAXIS_PID_IDX].I = 0.0;
  pid[ATTITUDE_GYRO_XAXIS_PID_IDX].D = -30.0;
  pid[ATTITUDE_GYRO_YAXIS_PID_IDX].P = 10.0;
  pid[ATTITUDE_GYRO_YAXIS_PID_IDX].I = 0.0;
  pid[ATTITUDE_GYRO_YAXIS_PID_IDX].D = -30.0;
  pid[RX_ROLL].P = 4;
  pid[RX_ROLL].I = 2;
  pid[RX_ROLL].D = 0;
  pid[RX_PITCH].P = 4;
  pid[RX_PITCH].I = 2;
  pid[RX_PITCH].D = 0;
  pid[RX_YAW].P = 4;
  pid[RX_YAW].I = 2;
  pid[RX_YAW].D = 0;
  pid[ATTITUDE_XAXIS_PID_IDX].P = 4.0;
  pid[ATTITUDE_XAXIS_PID_IDX].I = 2.0;
  pid[ATTITUDE_XAXIS_PID_IDX].D = 0.0;
  pid[ATTITUDE_YAXIS_PID_IDX].P = 4.0;
  pid[ATTITUDE_YAXIS_PID_IDX].I = 2.0;
  pid[ATTITUDE_YAXIS_PID_IDX].D = 0.0;
 
}




/*
  
 // Modified from http://www.arduino.cc/playground/Main/BarebonesPIDForEspresso
int updatePID(int targetPosition, int currentPosition, struct PID *p) {
  int error = targetPosition - currentPosition;  

  p->integratedError += error;

  // Limit the integrated error by the windupGuard
  if (p->integratedError < -windupGuard) {
    p->integratedError = -windupGuard;
  } else if (p->integratedError > windupGuard) {
    p->integratedError = windupGuard;
  }
  
  int dTerm = (p->D * (currentPosition - p->lastPosition)) / 10;
  p->lastPosition = currentPosition;
  return ((p->P * error) / 10) + ((p->I * p->integratedError) / 10) + dTerm;
}

void zeroIntegralError() {
  for(int n=0;n<3;n++) {
    pid[n].integratedError = 0;
  }
}
*/
